#include "system.h"
#include "ch375b.h"

int dly(){
  unsigned int t0 = getMills();
  while(getMills()-t0<10);
}

void setCMD(int cmd)
{
  dly();IOWR(CH375B, CH375B_WR, 1);
  dly();IOWR(CH375B, CH375B_A0, CMD);//A0 当A0=1 时可以写命令，当A0=0 时可以读写数据
  dly();IOWR(CH375B, CH375B_DAT, cmd);
  dly();IOWR(CH375B, CH375B_WR, 0);//WR#
  dly();IOWR(CH375B, CH375B_WR, 1);//WR#
}

void setDATA(int data)
{
  dly();IOWR(CH375B, CH375B_WR, 1);
  dly();IOWR(CH375B, CH375B_A0, DATA);//A0 当A0=1 时可以写命令，当A0=0 时可以读写数据
  dly();IOWR(CH375B, CH375B_DAT, data);
  dly();IOWR(CH375B, CH375B_WR, 0);//WR#
  dly();IOWR(CH375B, CH375B_WR, 1);//WR#
}
int getDATA()
{
  dly();IOWR(CH375B, CH375B_A0, DATA);//A0 当A0=1 时可以写命令，当A0=0 时可以读写数据
  dly();IOWR(CH375B, CH375B_RD, 0);//RD#
  dly();int val = IORD(CH375B, CH375B_DAT);
  dly();IOWR(CH375B, CH375B_RD, 1);//RD#
  return val;
}
int getDATA4()
{
  int val = IORD(CH375B, CH375B_DAT4);
  return val;
}

void setMode(int mode)
{
    setCMD(SET_USB_MODE);
    setDATA(mode);
}


int waitForInt()
{
  unsigned int t0 = getMills();
  while (true)
  {
    if((getMills() - t0) > 1000){
      return -1;
      break;
    }
    int tmp = IORD(CH375B, CH375B_DAT);
    if ((tmp & 0x100) == 0)
    {
      break;
    }
  }

  setCMD(GET_STATUS);
  int val = getDATA();
  return val;
}


int _readBlock(StorageDevice* self, unsigned int block, char* dst) {
  int* idst = (int*)dst;//0~127
  int val = 0;
  dly();IOWR(CH375B, CH375B_CS, 0);//CS#
  //该命令从USB 存储设备读取数据块。读取数据块以扇区为基本单位，需要两组参数：起始地址和数据长度，
  //起始地址是以4个字节表示的线性扇区号LBA 地址，数据长度是以1个字节表示的扇区数。
  //该命令需要5 个输入数据，依次是LBA 地址的最低字节，LBA地址的较低字节，LBA地址的较高字节，
  //LBA 地址的最高字节，扇区数。该命令可以在容量高达2000GB 的USB 存储设备中任意读取1 至255
  //个扇区的数据。该命令需要与下面的DISK_RD_GO命令配合使用。

  setCMD(DISK_READ);
  setDATA(block & 0xFF);
  setDATA((block >> 8) & 0xFF);
  setDATA((block >> 16) & 0xFF);
  setDATA((block >> 24) & 0xFF);
  setDATA(1);//扇区数 1~155

  val = waitForInt();
  if(val==-1){
    dly();IOWR(CH375B, CH375B_CS, 1);//CS#
    return 0;
  }

  for (int i = 0; i < 8; i++)
  {
    if (i != 0)
    {
      setCMD(DISK_RD_GO);
      val = waitForInt();
      if(val==-1){
        dly();IOWR(CH375B, CH375B_CS, 1);//CS#
        return 0;
      }
    }
    setCMD(RD_USB_DATA);
    int len = getDATA();
    for (int j = 0; j < 16; j++)
    {
        int temp = getDATA4();
        idst[i*16 + j] = temp;
        //data[i * 16 + j] = temp;
    }
  }
  setCMD(DISK_RD_GO);
  val = waitForInt();
  if(val==-1){
    dly();IOWR(CH375B, CH375B_CS, 1);//CS#
    return 0;
  }
  dly();IOWR(CH375B, CH375B_CS, 1);//CS#
  return 1;
}

int _writeBlock(StorageDevice* self, unsigned int blockNumber, const char* src) {
  return 0;
}

int CH375b::init() {
  initError = 0;

  this->readBlock = _readBlock;
  this->writeBlock = _writeBlock;

  //ver
  dly();IOWR(CH375B, CH375B_CS, 0);//CS#

  setCMD(RESET_ALL);

  //Thread.Sleep(100);
  int t0 = getMills();
  while(getMills()-t0<100);

  setCMD(GET_IC_VER);

  int ver = getDATA();
  //print("ver:");printInt(ver);print("\r\n");
  if(ver!=0x1b7){
    dly();IOWR(CH375B, CH375B_CS, 1);//CS#
    initError = 1;
    return 0;
  }

  setCMD(CHECK_EXIST);
  setDATA(0x55);

  int res = getDATA();
  if(res!=0x1aa){
    dly();IOWR(CH375B, CH375B_CS, 1);//CS#
    initError = 2;
    return 0;
  }

  
  setMode(5);//建议在没有USB 设备时使用模式5，插入USB 设备后先进入模式7再换到模式6。

  int val = waitForInt();
  if (val != 0x115){//15H USB_INT_CONNECT 检测到USB设备连接事件
    dly();IOWR(CH375B, CH375B_CS, 1);//CS#
    initError = 3;
    return 0;
  }

  setMode(7);

  setMode(6);
  val = waitForInt();
  if (val != 0x115){//15H USB_INT_CONNECT 检测到USB设备连接事件
    dly();IOWR(CH375B, CH375B_CS, 1);//CS#
    initError = 4;
    return 0;
  }

  setCMD(DISK_INIT);

  val = waitForInt();
  if (val != 0x114){
    dly();IOWR(CH375B, CH375B_CS, 1);//CS#
    initError = 5;
    return 0;
  }

  setCMD(DISK_SIZE);

  val = waitForInt();

  setCMD(RD_USB_DATA);

  //前4个字节以高字节在前组成的双字数据是USB 存储设备的总扇区数，
  //后4个字节以高字节在前组成的双字数据是每个扇区的字节数，
  //两个数据相乘的结果就是以字节为单位的USB存储设备的总容量。
  val = getDATA();
  if ((val & 0xFF) != 8) {
    dly();IOWR(CH375B, CH375B_CS, 1);//CS#
    initError = 6;
    return 0;
  }
  secCount = 0;
  int val2 = getDATA();
  secCount |= (val2 & 0xFF) << 24;
  int val3 = getDATA();
  secCount |= (val3 & 0xFF) << 16;
  int val4 = getDATA();
  secCount |= (val4 & 0xFF) << 8;
  int val5 = getDATA();
  secCount |= (val5 & 0xFF);

  secSize = 0;
  int val6 = getDATA();
  secSize |= (val6 & 0xFF) << 24;
  int val7 = getDATA();
  secSize |= (val7 & 0xFF) << 16;
  int val8 = getDATA();
  secSize |= (val8 & 0xFF) << 8;
  int val9 = getDATA();
  secSize |= (val9 & 0xFF);
  
  dly();IOWR(CH375B, CH375B_CS, 1);//CS#

  return 1;
}
